#pragma once

// write by lbf_heu 2023/04/16
// target 对点进行优化使其落在轨迹上

#include "path.hpp"
#include "robot.hpp"


#include <ceres/ceres.h>

Eigen::Matrix3d skew(Eigen::Vector3d _in){
    Eigen::Matrix3d res;
    res << 0., -_in(2), _in(1),
            _in(2), 0., -_in(0),
            -_in(1), _in(0), 0.;
    return res;
}

// Eigen::Quaterniond deltaQ(const Eigen::Vector3d &theta)
// {
//     Eigen::Quaterniond dq;
//     Eigen::Matrix<double, 3, 1> half_theta = theta;
//     half_theta /= (2.0);
//     dq.w() = (1.0);
//     dq.x() = half_theta.x();
//     dq.y() = half_theta.y();
//     dq.z() = half_theta.z();
//     return dq;
// }

// // 定义四元数类型
// class PoseLocalParameterization : public ceres::LocalParameterization
// {
//     virtual int GlobalSize() const { return 7; };
//     virtual int LocalSize() const { return 6; };
//     virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const
//     {
//         Eigen::Map<const Eigen::Vector3d> _p(x);
//         Eigen::Map<const Eigen::Quaterniond> _q(x + 3);

//         Eigen::Map<const Eigen::Vector3d> dp(delta);

//         Eigen::Quaterniond dq = deltaQ(Eigen::Map<const Eigen::Vector3d>(delta + 3));

//         Eigen::Map<Eigen::Vector3d> p(x_plus_delta);
//         Eigen::Map<Eigen::Quaterniond> q(x_plus_delta + 3);

//         p = _p + dp;
//         q = (_q * dq).normalized();

//         return true;
//     }
//     virtual bool ComputeJacobian(const double *x, double *jacobian) const
//     {
//         Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
//         j.topRows<6>().setIdentity();
//         j.bottomRows<1>().setZero();

//         return true;
//     }
// };

// 代价方程是点到线的距离
class myCostFunction : public ceres::SizedCostFunction<1, 3> {
	public:
		myCostFunction(robots * _robot, Eigen::Vector3d _line, double _dis, int _laserId){
            mpRobot = _robot;
            mLine = _line;
            mDis = _dis;
            mLaserId = _laserId;// 用于选择对应的外参
        }
		virtual ~myCostFunction() {}

		virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {    

            Eigen::Vector2d ti(parameters[0][0], parameters[0][1]);
            double theta = parameters[0][2];

            Eigen::Matrix3d ri; 
            ri << cos(theta), -sin(theta), 0., sin(theta), cos(theta), 0., 0., 0., 1.;
            ri.block<2, 1>(0, 2) = ti;
            auto robo = dynamic_cast<threeWheelRobots*>(mpRobot);// 转换成实体类型
            // 计算点到线的距离
            Eigen::Vector3d pInLaser(mDis, 0., 0.);
            Eigen::Vector3d pInRobot = robo->mLaserPos[mLaserId] * pInLaser + robo->mLaserTrans[mLaserId];// 激光坐标系下的测量转换到机器人坐标系
            pInRobot[2] = 1.;// 转变成齐次的情况
            Eigen::Vector3d pInWorld = ri * pInRobot;
            *residuals = mLine.transpose() * pInWorld;

            if(jacobians != NULL)// 雅可比矩阵是 1×3的
            {
                if(jacobians[0] != NULL)// 对位姿的导数
                { 
                    // 对pi的导数
                    jacobians[0][0] = mLine[0];
                    jacobians[0][1] = mLine[1];
                    Eigen::Vector2d temp;
                    double x = pInRobot[0];
                    double y = pInRobot[1];
                    temp << -x * sin(theta) - y * cos(theta), x * cos(theta) - y * sin(theta);
                    jacobians[0][2] = mLine.block<2,1>(0, 0).transpose() * temp;
                }
            }  

            return true;
        
        }   
		
        robots * mpRobot;
        Eigen::Vector3d mLine;
        double mDis;
        int mLaserId;// 用于选择对应的外参
};

// 由3自由度的运动 转换成 旋转矩阵
Eigen::Matrix3d se2TwoRotMatrix(const Eigen::Vector3d & rotVec){
    Eigen::Matrix3d res;
    res << cos(rotVec[2]), -sin(rotVec[2]), rotVec[0], sin(rotVec[2]), cos(rotVec[2]), rotVec[1], 0., 0., 1.;
    return res;
}
// 代价方程是点到线的距离  残差1维度  定位点  激光的外参
class psExtCostFunction : public ceres::SizedCostFunction<1, 3, 3> {
	public:
		psExtCostFunction(robots * _robot, Eigen::Vector3d _line, double _dis){
            mpRobot = _robot;// 机器人指针
            mLine = _line;// 墙的情况
            mDis = _dis;// 测量得到的距离
        }
		virtual ~psExtCostFunction() {}

		virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {    
            // 坐标位姿参数
            Eigen::Vector3d t_w_r(parameters[0][0], parameters[0][1], parameters[0][2]);// robot 2 world
            // 外参参数
            Eigen::Vector3d t_r_l(parameters[1][0], parameters[1][1], parameters[1][2]);// laser 2 world
            // 转换成旋转矩阵形式的
            Eigen::Matrix3d T_w_r = se2TwoRotMatrix(t_w_r); 
            Eigen::Matrix3d T_r_l = se2TwoRotMatrix(t_r_l); 

            auto robo = dynamic_cast<threeWheelRobots*>(mpRobot);// 转换成实际类型
            // 计算点到线的距离
            Eigen::Vector3d pInLaser(mDis, 0., 1.);// 激光测量到的数据的2d齐次坐标
            Eigen::Vector3d pInRobot = T_r_l * pInLaser;// 激光坐标系下的测量转换到机器人坐标系
            Eigen::Vector3d pInWorld = T_w_r * pInRobot;// 转换成世界坐标系下的
            *residuals = mLine.transpose() * pInWorld;// 求解残差

            if(jacobians != NULL)// 雅可比矩阵是 1×3的
            {
                if(jacobians[0] != NULL)// 对位姿参数进行计算
                { 
                    // 对pi的导数
                    jacobians[0][0] = mLine[0];
                    jacobians[0][1] = mLine[1];
                    Eigen::Vector2d temp;
                    double x = pInRobot[0];
                    double y = pInRobot[1];
                    double theta = parameters[0][2];
                    temp << -x * sin(theta) - y * cos(theta), x * cos(theta) - y * sin(theta);
                    jacobians[0][2] = mLine.block<2,1>(0, 0).transpose() * temp;
                }
                if(jacobians[1] != NULL)// 对外参的导数
                {
                    Eigen::Vector3d temp =  mLine.transpose() * T_w_r;
                    double theta = parameters[1][2];
                    jacobians[1][0] = temp[0];
                    jacobians[1][1] = temp[1];
                    jacobians[1][2] = -temp[0] * mDis * sin(theta) +  temp[1] * mDis * cos(theta);
                }
            }  

            return true;
        
        }   
		
        robots * mpRobot;
        Eigen::Vector3d mLine;
        double mDis;
};

// 代价方程是点到线的距离  残差1维度  定位点  激光的外参 x y theta
class copyExtCostFunction : public ceres::SizedCostFunction<1, 3, 2, 1> {
	public:
		copyExtCostFunction(robots * _robot, Eigen::Vector3d _line, double _dis){
            mpRobot = _robot;// 机器人指针
            mLine = _line;// 墙的情况
            mDis = _dis;// 测量得到的距离
        }
		virtual ~copyExtCostFunction() {}

		virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {    
            // 坐标位姿参数
            Eigen::Vector3d t_w_r(parameters[0][0], parameters[0][1], parameters[0][2]);// robot 2 world
            // 外参参数
            Eigen::Vector3d t_r_l(parameters[1][0], parameters[1][1], parameters[2][0]);// laser 2 world
            // 转换成旋转矩阵形式的
            Eigen::Matrix3d T_w_r = se2TwoRotMatrix(t_w_r); 
            Eigen::Matrix3d T_r_l = se2TwoRotMatrix(t_r_l); 

            auto robo = dynamic_cast<threeWheelRobots*>(mpRobot);// 转换成实际类型
            // 计算点到线的距离
            Eigen::Vector3d pInLaser(mDis, 0., 1.);// 激光测量到的数据的2d齐次坐标
            Eigen::Vector3d pInRobot = T_r_l * pInLaser;// 激光坐标系下的测量转换到机器人坐标系
            Eigen::Vector3d pInWorld = T_w_r * pInRobot;// 转换成世界坐标系下的
            *residuals = mLine.transpose() * pInWorld;// 求解残差

            if(jacobians != NULL)// 雅可比矩阵是 1×3的
            {
                if(jacobians[0] != NULL)// 对位姿参数进行计算
                { 
                    // 对pi的导数
                    jacobians[0][0] = mLine[0];
                    jacobians[0][1] = mLine[1];
                    Eigen::Vector2d temp;
                    double x = pInRobot[0];
                    double y = pInRobot[1];
                    double theta = parameters[0][2];
                    temp << -x * sin(theta) - y * cos(theta), x * cos(theta) - y * sin(theta);
                    jacobians[0][2] = mLine.block<2,1>(0, 0).transpose() * temp;
                }
                if(jacobians[1] != NULL)// 对外参的导数
                {
                    Eigen::Vector3d temp =  mLine.transpose() * T_w_r;
                    double theta = parameters[2][0];
                    jacobians[1][0] = temp[0];
                    jacobians[1][1] = temp[1];
                    
                }
                if(jacobians[2] != NULL)
                {
                    Eigen::Vector3d temp =  mLine.transpose() * T_w_r;
                    double theta = parameters[2][0];
                    jacobians[2][0] = -temp[0] * mDis * sin(theta) +  temp[1] * mDis * cos(theta);
                }
            }  

            return true;
        
        }   
		
        robots * mpRobot;
        Eigen::Vector3d mLine;
        double mDis;
};

class opt
{
public:

    robots * mpRobot;

    opt(robots * _Robot){
        mpRobot = _Robot;
    }
    // 对位姿进行优化
    void optPosAndTrans(Eigen::Vector3d & _ps, std::vector<double> _lasersMes, std::vector<Eigen::Vector3d> _Walls){// 2维度优化
        // 构建问题
        ceres::Problem::Options problem_options;
        ceres::Problem problem(problem_options);
        // 机器人的位姿
        double* parame = new double[3];
        parame[0] = _ps[0];
        parame[1] = _ps[1];
        parame[2] = _ps[2];

        // 添加参数块
        // ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
        problem.AddParameterBlock(parame, 3);// 在se2的李代数上进行优化
        // 添加残差块
        auto robo  = dynamic_cast<threeWheelRobots*>(mpRobot);
        if((_Walls.size() != _lasersMes.size())){
            std::cout << "size error !!!  _Walls = " << _Walls.size() << " lasers = " << _lasersMes.size() << std::endl;
            return;
        }
        for(size_t i = 0; i < robo->mLaserPos.size(); ++i){// 对每个激光进行计算
            ceres::CostFunction *cost_function = new myCostFunction(mpRobot, _Walls[i],_lasersMes[i], i);  
            problem.AddResidualBlock(cost_function, NULL, parame);
        }
        // 优化
        ceres::Solver::Options options;
        options.linear_solver_type = ceres::DENSE_QR;
        options.minimizer_progress_to_stdout = false;
        options.check_gradients = false;
        options.gradient_check_relative_precision = 1e-4;
        ceres::Solver::Summary summary;
        ceres::Solve(options, &problem, &summary);
        // 传递优化出来的
        _ps << parame[0], parame[1], parame[2];
        delete [] parame;
    }
    // 对外参以及位姿进行优化
    void optPosAndTransAndEXt(std::vector<Eigen::Vector3d> & _Walls, // 墙壁的参数
                              std::vector<std::vector<Eigen::Vector9d>> & _Vlasers, // n组激光的数据
                              std::vector<Eigen::Vector3d> & _ptVec, // 待优化的位姿参数
                              std::vector<Eigen::Vector3d> & _laserExt // 待优化的外参参数 
                              ){
        // 构建问题
        ceres::Problem::Options problem_options;
        ceres::Problem problem(problem_options);
        // 机器人的位姿  并开辟空间
        std::vector<double *> paramVec(_Vlasers.size());
        for(int i = 0; i < paramVec.size(); ++i){
            paramVec[i] = new double[3];
            paramVec[i][0] = _ptVec[i][0];
            paramVec[i][1] = _ptVec[i][1];
            paramVec[i][2] = _ptVec[i][2];

            problem.AddParameterBlock(paramVec[i], 3);// 加入n多个优化参数
        }

        // 加入外参的优化块
        std::vector<double *> paramExt(_laserExt.size());
        for(int i = 0; i < paramExt.size(); i++){
            paramExt[i] = new double[3];
            paramExt[i][0] = _laserExt[i][0];
            paramExt[i][1] = _laserExt[i][1];
            paramExt[i][2] = _laserExt[i][2];
            // problem.AddParameterBlock(paramExt[i], 3);// 加入n多个优化参数
            // problem.SetParameterBlockConstant(paramExt[i]);// 外参fix
            problem.AddParameterBlock(paramExt[i], 2);// 加入n多个优化参数
            problem.AddParameterBlock(paramExt[i]+2, 1);// 加入n多个优化参数
            problem.SetParameterBlockConstant(paramExt[i]);// 外参fix
            problem.SetParameterBlockConstant(paramExt[i]+2);// 外参fix
        }
        
        // 加入所有的残差
        for(size_t i = 0; i < _Vlasers.size(); ++i){
            std::vector<Eigen::Vector9d> & lasers = _Vlasers[i];
            if(lasers.size() != 4) continue;
            for(size_t j = 0; j < lasers.size(); ++j){
                double dis = lasers[j](3, 0);
                // ceres::CostFunction *cost_function = new psExtCostFunction(mpRobot, _Walls[j], dis);  
                // problem.AddResidualBlock(cost_function, NULL, paramVec[i], paramExt[j]);
                ceres::CostFunction *cost_function = new copyExtCostFunction(mpRobot, _Walls[j], dis);  
                problem.AddResidualBlock(cost_function, NULL, paramVec[i], paramExt[j], paramExt[j]+2);
            }
            
        }
        
        // 调用优化器进行优化
        ceres::Solver::Options options;
        options.linear_solver_type = ceres::DENSE_QR;
        options.minimizer_progress_to_stdout = true;
        options.check_gradients = false;
        options.max_num_iterations = 15;
        options.gradient_check_relative_precision = 1e-4;
        ceres::Solver::Summary summary;
        ceres::Solve(options, &problem, &summary);

        // copy回来 释放空间
        for(int i = 0; i < paramVec.size(); ++i){// 释放优化点参数块
            _ptVec[i][0] = paramVec[i][0];
            _ptVec[i][1] = paramVec[i][1];
            _ptVec[i][2] = paramVec[i][2];
            delete[] paramVec[i];
        }

        for(int i = 0; i < paramExt.size(); i++){// 释放外参参数块
            _laserExt[i][0] = paramExt[i][0];
            _laserExt[i][1] = paramExt[i][1];
            _laserExt[i][2] = paramExt[i][2];
            delete[] paramVec[i];
        }
    }
    // 检查雅可比和残差
    void chackErrorAndJacobian(){
        Eigen::Vector3d line(1., 2., 3.);
        psExtCostFunction costFun(mpRobot, line, 0.4);
        // 计算数值导数与解析导数
        double** parameters = new double*[2];// 两个参数
        parameters[0] = new double[3];
        parameters[0][0] = 0.5;
        parameters[0][1] = 0.5;
        parameters[0][2] = 0;

        parameters[1] = new double[3];
        parameters[1][0] = 0.3;
        parameters[1][1] = 0.3;
        parameters[1][2] = 0.1;

        double *residuals = new double;
        double ** jacobians = new double*[2];
        jacobians[0] = new double[3];
        jacobians[1] = new double[3];
        costFun.Evaluate(parameters, residuals, jacobians);
        // 打印残差与雅可比矩阵
        std::cout << "param 1" << "(" << parameters[0][0] << "," << parameters[0][1] << "," << parameters[0][2] << ")" << std::endl;
        std::cout << "param 2" << "(" << parameters[1][0] << "," << parameters[1][1] << "," << parameters[1][2] << ")" << std::endl;
        std::cout << "res = " << *residuals << std::endl;

        std::cout << "jacobian 1" << "(" << jacobians[0][0] << "," << jacobians[0][1] << "," << jacobians[0][2] << ")" << std::endl;
        std::cout << "jacobian 2" << "(" << jacobians[1][0] << "," << jacobians[1][1] << "," << jacobians[1][2] << ")" << std::endl;

        // 进行解析导数
        double* residualNow = new double;
        double** parametersNew = new double*[2];
        parametersNew[0] = new double[3];
        parametersNew[1] = new double[3];
        double eps = 1e-6;
        Eigen::Vector6d jacobianNow;
        for(int i = 0; i < 2; i++)
        for(int j = 0; j < 3; j++){
            for(int m = 0; m < 2; m++) for(int n = 0; n < 3; n++) parametersNew[m][n] = parameters[m][n];
            parametersNew[i][j] += eps;
            costFun.Evaluate(parametersNew, residualNow, NULL);
            jacobianNow(i*3+j, 0) = (*residualNow - *residuals)/eps;
        }
        std::cout << jacobianNow.transpose() << std::endl;
    }
    ~opt(){}
};



